#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <opencv/cxcore.h>
/*#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/objdetect/objdetect.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <string.h>
#include <ctype.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <math.h>
#include <float.h>
#include <limits.h>
#include <time.h>*/

#include "rovio.h"

// Create memory for calculations
static CvMemStorage* storage = 0;

// Create a new Haar classifier
static CvHaarClassifierCascade* cascade = 0;

using namespace cv;
using namespace std;

struct DriveCommand {
	DriveDirection dir;
	int time;
};

// Function prototype for detecting and drawing an object from an image
DriveCommand detect_and_draw(float threshold, IplImage* image);

// Create a string that contains the cascade name
const char* cascade_name = "haarcascade_frontalface_alt.xml";

DriveCommand determineFaceCommand(float threshold, cv::Mat camImage)
{
	IplImage image;
	image = camImage;

	// Load the HaarClassifierCascade
	cascade = (CvHaarClassifierCascade*)cvLoad( cascade_name, 0, 0, 0 );

 	// Check whether the cascade has loaded successfully.
 	// Else report and error and quit
    if(!cascade) {
		fprintf( stderr, "ERROR: Could not load classifier cascade\n" );
		DriveCommand cmd;
		cmd.dir = DIR_STOP;
		cmd.time = 0;
		return cmd;
    }

	// Allocate the memory storage
	storage = cvCreateMemStorage(0);
	
	// Call the function to detect and draw the face
	return detect_and_draw(threshold, &image);
}

DriveCommand detect_and_draw(float threshold, IplImage* img)
{
	int scale = 1;
	DriveCommand cmd;
	cmd.time = 100;

	// Create a new image based on the input image
	CvSize size = cvSize(img->width / scale, img->height / scale);
	IplImage* temp = cvCreateImage(size, 8, 3);

	// Create two points to represent the face locations
	CvPoint pt1, pt2;
	int i;

	// Clear the memory storage which was used before
	cvClearMemStorage(storage);

	// Find whether the cascade is loaded, to find the faces. If yes, then:
	if (!cascade) {
		DriveCommand cmd;
		cmd.dir = DIR_STOP;
		cmd.time = 0;
		return cmd;
	}
	
	// There can be more than one face in an image. So create a growable sequence of faces.
	// Detect the objects and store them in the sequence
	CvSeq* faces = cvHaarDetectObjects(img, cascade, storage, 1.1, 2,
			CV_HAAR_DO_CANNY_PRUNING,
			cvSize(40, 40));
	
	// Loop the number of faces found.
	for(i = 0; i < (faces ? faces->total : 0); i++ )
	{
		// Create a new rectangle for drawing the face
		CvRect* r = (CvRect*)cvGetSeqElem( faces, i );
		
		// Find the dimensions of the face,and scale it if necessary
		pt1.x = r->x*scale;
		pt2.x = (r->x+r->width)*scale;
		pt1.y = r->y*scale;
		pt2.y = (r->y+r->height)*scale;
		
		// Draw the rectangle in the input image
		cvRectangle( img, pt1, pt2, CV_RGB(255,0,0), 3, 8, 0 );
	}
    
    std::cout << faces->total << std::endl;
    
    if(faces->total == 0) {
        cmd.dir = DIR_ROT_RIGHT;
        cmd.time = 300;
    } else {
        CvRect* r = (CvRect*)cvGetSeqElem( faces, 0);
        
        //get the center point of the face
        pt1.x = r->x + r->width / 2;
        pt1.y = r->y + r->height / 2;
        
        if(abs(pt1.x - img->width / 2) < threshold * img->width) {
			cmd.dir = (DIR_FORWARD);
			//cmd.time = 1000;
			double distance = -0.1111 * r->height + 10.333;
			cout << "distance: " << distance << endl;
			
			static int millisPerFoot = 750;
			static int maxTime = 5000;
			static int minTime = 1000;
			
			cmd.time = millisPerFoot * distance;
			
			if (cmd.time > maxTime) {
				cmd.time = maxTime;
			} else if (cmd.time < minTime) {
				cmd.time = minTime;
			}
			cout << "time" << cmd.time << endl;
        } else if(pt1.x < img->width / 2) {
			cmd.dir = (DIR_ROT_LEFT);
		} else {
			cmd.dir = (DIR_ROT_RIGHT);
		}
	}

	// Show the image in the window named "result"
	cvShowImage( "FaceFinder", img );
	
	// Release the temp image created.
	cvReleaseImage( &temp );
    
    return cmd;
}

int main(int argc, char **argv) {
	Mat image;
	
	while (true) {
//		Rovio rovio("http://192.168.10.18");
		Rovio rovio("http://192.168.10.18");
		
		if (argc > 1) {
			std::stringstream stream;
			stream << "rovio_img_";
			stream << argv[1];
			stream << ".jpg";
			
			image = imread(stream.str().c_str(), 1);
		} else {
			rovio.saveImage("rovio_img.jpg");
			image = imread("rovio_img.jpg", 1);
		}
		
		if (image.empty()) {
			std::cerr << "Image did not load" << std::endl;
		} else {		
			DriveCommand command = determineFaceCommand(.2, image);
			std::cout << "Drive direction: " << command.dir << std::endl;
			
			if (command.dir == DIR_FORWARD) {
				rovio.timeDrive(command.dir, 1000);
			} else {
				rovio.timeDrive(command.dir, command.time);
			}
			
			rovio.drive(DIR_STOP, 250);
		}
		
		if (cvWaitKey(100) == 27) {
			break;
		}
	}
}

